using System;
using Microsoft.SPOT;

namespace UAV.Drivers
{
    public static class GeoDistanceCalculator
    {
        private const double _earthRadiusInMiles = 3956.0;
        private const double _earthRadiusInKilometers = 6367.0;
        public static double DistanceInMiles(double lat1, double lng1, double lat2, double lng2)
        {
            return Distance(lat1, lng1, lat2, lng2, _earthRadiusInMiles);
        }
        public static double DistanceInKilometers(double lat1, double lng1, double lat2, double lng2)
        {
            return Distance(lat1, lng1, lat2, lng2, _earthRadiusInKilometers);
        }
        private static double Distance(double lat1, double lng1, double lat2, double lng2, double radius)
        {
            // Implements the Haversine formulat http://en.wikipedia.org/wiki/Haversine_formula
            //
            var lat = MathMF.ToRadians(lat2 - lat1);
            var lng = MathMF.ToRadians(lng2 - lng1);
            var sinLat = MathMF.Sin(0.5 * lat);
            var sinLng = MathMF.Sin(0.5 * lng);
            var cosLat1 = MathMF.Cos(MathMF.ToRadians(lat1));
            var cosLat2 = MathMF.Cos(MathMF.ToRadians(lat2));
            var h1 = sinLat * sinLat + cosLat1 * cosLat2 * sinLng * sinLng;
            var h2 = MathMF.Sqrt(h1);
            var h3 = 2 * MathMF.Asin(MathMF.Min(1, h2));
            return radius * h3;
        }
    }
}
